//
// Created by 17616 on 2021/4/22.
//
#include"CLI.h"
#include"stm32_hal.h"
#include"../c_inv_imu/drv_imu_invensense.h"
extern I2C_HandleTypeDef hi2c1;
extern SPI_HandleTypeDef hspi1;
extern SPI_HandleTypeDef hspi2;

int IMU_INV_I2C_TransferBlocking(const inv_i2c_transfer_t *tf) {
    if (tf->direction == inv_i2c_direction_Read) {
        return HAL_I2C_Mem_Read(&hi2c1, tf->slaveAddress,
                                tf->subAddress|1, tf->subAddressSize, (uint8_t *) tf->data, tf->dataSize, ~0);
    } else {
        return HAL_I2C_Mem_Write(&hi2c1, tf->slaveAddress,
                                 tf->subAddress, tf->subAddressSize, (uint8_t *) tf->data, tf->dataSize, ~0);
    }
}

int IMU_INV_SPI_TransferBlocking(const inv_spi_transfer_t *tf) {
    return HAL_SPI_TransmitReceive(&hspi1, tf->txData, tf->rxData, tf->dataSize, ~0);
}
int imu_test(shell::CLI &cli, const std::vector<const char *> &args) {
    inv_i2c_t i2c0 =
            {
                    .masterTransferBlocking = IMU_INV_I2C_TransferBlocking,
                    .masterTransferNonBlocking = IMU_INV_I2C_TransferBlocking,
            };
    inv_spi_t spi0 = {
            .masterTransferBlocking = IMU_INV_SPI_TransferBlocking,
            .masterTransferNonBlocking = IMU_INV_SPI_TransferBlocking,
    };
    inv_imu_handle_t imu0;
    if (args.size() > 0 && (std::string(args[0]) == "spi")) {
        imu0 = IMU_AutoConstructSPI(spi0);
        cli.printf("spi\r\n");
    } else {
        imu0 = IMU_AutoConstructI2C(i2c0, IMU_SlaveAddressAutoDetect);
    }
    if (imu0 != NULL) {
        cli.printf("当前使用的传感器为 %s\r\n", IMU_Report(imu0));
        if (0 == IMU_Init(imu0, IMU_ConfigDefault())) {
            cli.printf("ST = %d,0为自检通过,自检时保持静止\r\n", IMU_SelfTest(imu0));
            // Delay(10)//delay 10ms
            int loop = 10;
            while (loop--) {
                IMU_ReadSensorBlocking(imu0);
                float temp;
                float data[9];
                float *buf = data;
                IMU_Convert(imu0, data);
                IMU_ConvertTemp(imu0, &temp);
                cli.printf("temp = %f\r\n", temp);
                cli.printf("accel(xyz) = %.3f %.3f %.3f\t gyro(xyz) = %.3f %.3f %.3f\t mag(xyz) = %.3f %.3f %.3f \r\n",
                           *buf++, *buf++, *buf++, *buf++, *buf++, *buf++, *buf++, *buf++, *buf++);
            }
        } else {
            cli.printf("初始化失败\r\n");
        }
    } else {
        cli.printf("imu0 == NULL，没接或者iic/spi读写出错\r\n");
    }
    if (imu0 != NULL) {
        cli.printf("释放资源\r\n");
        IMU_Destruct(imu0);
    }
    return 0;
}
shell::CLI::CMD __inv_imu_testetstetts("imu", imu_test);
